#ifndef CAROS_DEMO_LEAPMOTION_H
#define CAROS_DEMO_LEAPMOTION_H

#include <Leap.h>
#include <caros/caros_node_service_interface.h>
#include <caros_sensor_msgs/PoseSensorState.h>
#include <caros_sensor_msgs/ButtonSensorState.h>
#include <ros/ros.h>
#include <caros/hand_detection/leap_hand_processing.h>
#include <caros/hand_detection/camera_processing.h>
#include <string>
#include <mutex>

namespace cv
{
class Mat;
}

namespace caros
{
class LeapMotionNode : public caros::CarosNodeServiceInterface, public Leap::Listener
{
 public:
  explicit LeapMotionNode(const ros::NodeHandle& nh, const std::string& name = "/demo_leapmotion");
  ~LeapMotionNode()
  {
    controller_.removeListener(*this);
  };

 public:  // Leap Listeners Runs in separate thread
  virtual void onConnect(const Leap::Controller&);
  virtual void onFrame(const Leap::Controller&);
  virtual void onImages(const Leap::Controller&);

 protected:             // CAROS STUFF
  bool activateHook();  // Is called when the node is started
  bool recoverHook(const std::string& error_msg, const int64_t error_code);

  void runLoopHook();         // The Main operation of this node is running in loop here
  void errorLoopHook();       // Not used
  void fatalErrorLoopHook();  // Not used

 private:
  void unDistortLeapImage(const Leap::Image& src, cv::Mat& dst);
  void findHand(const Leap::HandList& hands);

  // Publishing information
  void noMeasurement();
  void addMeasurement(const Leap::Vector& pos, const Quaternion& rot, double quality = 0);
  void setGrab(const double & value);
  void setSphereGrab(const double & value);
  caros_sensor_msgs::PoseSensorState pose_msg_;
  caros_sensor_msgs::ButtonSensorState button_msg_;
  ros::Publisher pose_pub_;
  ros::Publisher button_pub_;
  std::mutex pose_guard_;
  std::mutex button_guard_;
  ros::Time button_stamp_;

  // ROS stuff
  ros::NodeHandle nh_;

  // Leap Motion stuff
  Leap::Controller controller_;
  CameraProcessing img_processer_;
  LeapHandProcess hand_processer_;

  // settings
  bool un_distort_image_;
  bool use_camera_;
};

}  // namespace caros
#endif